!------------------------------------------------------------------------------
! Copyright: qinguoming
!------------------------------------------------------------------------------
!
! MODULE:  LS_mod
!
!> @author
!> qin guoming
!
! DESCRIPTION: 
!>  This module contains the subroutines for system solving.
!
!------------------------------------------------------------------------------
module ls_mod
    use iso_fortran_env, only:real64,int64
    use sconstants_mod
    use bcs_mod
    use matrix_utils_mod
    implicit none
contains
    ! 针对壳单元进行线性静力分析,默认总时间为1.0,步长为1.0,载荷为线性增量(Ramp) 
    subroutine do_shell_static_solve(nodes,el_conn,e, nu, t,  &
                                    dbc_pos, dbc_val, &
                                    fbc_pos, fbc_val, &
                                    gdof, nodal_u, nodal_force)
        use shell_mod
        implicit none
        ! 输入变量声明
        real(real64),intent(in)         :: e,t,nu
        real(real64),intent(in)         :: nodes(:,:)
        real(real64),intent(in)         :: dbc_val(:)
        real(real64),intent(in)         :: fbc_val(:)
        integer,intent(in)              :: el_conn(:,:),dbc_pos(:,:),fbc_pos(:,:)
        integer,intent(in)              :: gdof
        ! 输出变量声明
        real(real64),intent(out)        :: nodal_u(:,:),nodal_force(:,:)
        ! 内部变量声明
        integer                         :: el_inds(elem_dof_num),nnum,ninds(node_dof_num)
        integer                         :: i,j,ok
        real(real64)                    :: el_coords(3,el_node_num)
        real(real64)                    :: elk(elem_dof_num,elem_dof_num)
        real(real64)                    :: gke(gdof, gdof)
        real(real64)                    :: gke_(gdof,gdof)
        real(real64)                    :: gu(gdof)
        real(real64)                    :: gfe_(gdof)
        real(real64)                    :: gfe(gdof)
        real(real64)                    :: bcs(size(dbc_val,1),2)
        real(real64)                    :: fbcs(size(fbc_val,1),2)
        
        nnum = size(nodes,1)
        print *,'global dof number          =',gdof
        print *,'node number                =',nnum
        ! 初始化
        i = 0
        j = 0
        el_coords   = 0.0_real64
        gke         = 0.0_real64
        gke_        = 0.0_real64
        gfe_        = 0.0_real64
        gfe         = 0.0_real64
        elk         = 0.0_real64
        print *, '====> Start assembling global stiffness matrix'
        do i = 1,size(el_conn,1)
            ! 获得单元节点坐标数组
            do j = 1,size(el_conn,2)
                el_coords(1,j) = nodes(el_conn(i,j),1)
                el_coords(2,j) = nodes(el_conn(i,j),2)
                el_coords(3,j) = nodes(el_conn(i,j),3)
            end do
            call get_gke(e,nu,t,el_coords,elk)              ! 获得单元刚度矩阵
            call get_eldof_inds(el_conn(i,:),el_inds)       ! 获得单元自由度索引
            gke(el_inds,el_inds)=gke(el_inds,el_inds)+elk   ! 组装全局刚度矩阵
        end do
        print *, '====> End assembling global stiffness matrix'

        ! 处理位移边界条件
        do i=1,size(dbc_val,1)
            ! 计算自由度索引和位移值,可能bcs(i,1)==bcs(j,1),但是不影响对刚度矩阵的处理
            bcs(i,1) = real(get_dof_ind(dbc_pos(i,1),dbc_pos(i,2)),real64)
            bcs(i,2) = dbc_val(i)
        end do
        print *, '====> processing dispalcement bcs done'

        ! 处理力加载边界条件
        do i=1,size(fbc_val,1)
            fbcs(i,1) = real(get_dof_ind(fbc_pos(i,1),fbc_pos(i,2)),real64)
            fbcs(i,2) = fbc_val(i)
        end do
        print *, '=====> processing force loads done'
        
        ! 将边界条件引入到刚度矩阵和力向量中
        call add_bcs_by_reorder(gke,bcs,fbcs,gke_,gfe_)
        print *,'=====> Bcs & Loads are introduce into K matrix done'

        ! 使用lapack库求解线性方程组
        ! 考虑到修改后的刚度矩阵一般也是对称正定矩阵,所以使用lapack的SPOSV driver求解
        print *,'=====> Start calling LAPACK to solve KU=F....'
        call soleq(gke_,gfe_,gdof,gu,ok)
        if (ok/=0) then 
            print *, 'solveq failed'
            stop 
        end if 
        ! 计算节点反力
        gfe=matmul(gke,gu)
        print *,'=====> solve Finished!'
        
        ! 输出结果
        print *,'=====> Start sorting results!'
        do i=1,nnum
            ninds=get_ndof_inds(i)
            nodal_u(i,:)=gu(ninds)
            nodal_force(i,:)=gfe(ninds)
        end do
        print *, '=====>  Get node displacements and forces done !'
    end subroutine do_shell_static_solve
    
    !> 针对实体单元进行线性静力分析, 默认总时间为1.0,步长为1.0,载荷为线性增量(Ramp) 
    subroutine do_hex_static_solve(nodes,el_conn,e,nu,elemtype,&
                                    dbc_pos, dbc_val,  &
                                    fbc_pos, fbc_val,  &
                                    gdof,nodal_u,nodal_force)
        use solid_mod,only: eldofs,nndofs,nn_el,get_dof_ind,get_ndof_inds,&
        get_eldof_inds,c3d8i_ke,c3d8_ke
        implicit none
        ! 输入变量声明
        integer,intent(in)          :: el_conn(:,:),dbc_pos(:,:),fbc_pos(:,:)
        integer,intent(in)          :: elemtype,gdof
        real(real64),intent(in)     :: e,nu
        real(real64),intent(in)     :: nodes(:,:), dbc_val(:),fbc_val(:)
        ! 输出变量声明
        real(real64),intent(out)    :: nodal_u(:,:),nodal_force(:,:)
        ! 内部变量声明
        integer                     :: i,j,el_inds(eldofs),nnum,ninds(nndofs),ok
        real(real64)                :: el_coords(3,nn_el)
        real(real64)                :: elk(eldofs,eldofs)
        real(real64)                :: gke(gdof, gdof)
        real(real64)                :: gke_(gdof,gdof)
        real(real64)                :: gu(gdof)
        real(real64)                :: gfe_(gdof)
        real(real64)                :: gfe(gdof)
        real(real64)                :: bcs(size(dbc_val,1),2)
        real(real64)                :: fbcs(size(fbc_val,1),2)

        nnum=size(nodes,1)
        print *,'global dof number          =',gdof
        print *,'node number                =',nnum
        ! 初始化
        i=0
        j=0
        el_coords   = 0.0_real64
        gke         = 0.0_real64
        gke_        = 0.0_real64
        gfe_        = 0.0_real64
        gfe         = 0.0_real64
        elk         = 0.0_real64
        print *, '====> Start assembling global stiffness matrix'
        do i=1,size(el_conn,1)
            ! 获得单元节点坐标数组
            do j=1,size(el_conn,2)
                el_coords(1,j)=nodes(el_conn(i,j),1)
                el_coords(2,j)=nodes(el_conn(i,j),2)
                el_coords(3,j)=nodes(el_conn(i,j),3)
            end do
            if (elemtype==1) then
                call c3d8_ke(e,nu,el_coords,elk)            ! 获得c3d8单元刚度矩阵
            else if (elemtype==2) then
                call c3d8i_ke(e,nu,el_coords,elk)           ! 获得c3d8i单元刚度矩阵
            end if
            call get_eldof_inds(el_conn(i,:),el_inds)       ! 获得单元自由度索引
            gke(el_inds,el_inds)=gke(el_inds,el_inds)+elk   ! 组装全局刚度矩阵
        end do
        print *, '====> End assembling global stiffness matrix'

        ! 处理位移边界条件
        bcs=0.0_real64
        do i=1,size(dbc_val,1)
            ! 计算自由度索引和位移值,可能bcs(i,1)==bcs(j,1),但是不影响对刚度矩阵的处理
            bcs(i,1)=real(get_dof_ind(dbc_pos(i,1),dbc_pos(i,2)),real64)
            bcs(i,2)=dbc_val(i)
        end do
        print *, '====> processing dispalcement bcs done'

        ! 处理力加载边界条件
        fbcs=0.0_real64
        do i=1,size(fbc_val,1)
            fbcs(i,1)=real(get_dof_ind(fbc_pos(i,1),fbc_pos(i,2)),real64)
            fbcs(i,2)=fbc_val(i)
        end do
        print *, '=====> processing force loads done'
        
        ! 将边界条件引入到刚度矩阵和力向量中
        call add_bcs_by_reorder(gke,bcs,fbcs,gke_,gfe_)
        print *,'=====> Bcs & Loads are introduce into K matrix done'
        
        ! 使用lapack库求解线性方程组
        ! 考虑到修改后的刚度矩阵一般也是对称正定矩阵,所以使用lapack的SPOSV driver求解
        print *,'=====> Start calling LAPACK to solve KU=F....'
        call soleq(gke_,gfe_,gdof,gu,ok) 
        if (ok/=0) then 
            print *, 'solveq failed'
            stop
        end if
        ! 计算节点反力
        gfe=matmul(gke,gu)
        print *,'=====> solve Finished!'
        
        ! 输出结果
        print *,'=====> Start sorting results!'
        do i=1,nnum
            ninds=get_ndof_inds(i)
            nodal_u(i,:)=gu(ninds)
            nodal_force(i,:)=gfe(ninds)
        end do
        print *, '=====>  Get node displacements and forces done !'
    end subroutine do_hex_static_solve

    !>使用多种方法求解KU=F
    subroutine soleq(gke,gf,gdof,gu,ok)
        real(real64), intent(in) :: gke(:,:),gf(:)
        integer, intent(in) :: gdof
        real(real64), intent(out) :: gu(:)
        integer,intent(out)::ok
        ! 内部变量声明
        integer         :: info,ipiv(gdof)
        real(real64)    :: gke_sol(gdof,gdof),gfp_sol(gdof)
        logical         :: flag

        gke_sol=gke
        gfp_sol=gf
        flag=.false.
        ok=1
        ! 优先使用SPOSV求解
        print *,'   Try solving KU=F by DPOSV....'
        if (.not. flag)then 
            call dposv('U',gdof,1,gke_sol,gdof,gfp_sol,gdof,info)
            if (info/=0)then
                flag=.false.
                ! 求解失败,将A,B重新赋值
                print *, '      KU=F solve by dposv failed !, info = ',info
                gke_sol=gke
                gfp_sol=gf
                ok=1
            else
                flag=.true.
                gu=gfp_sol
                ok=0
                print *, '      KU=F solved by dposv successfully !'
            end if
        end if
        print *,'   Try solving KU=F by dgesv....'
        ! 其次使用dgesv求解
        if (.not. flag)then
            call dgesv(gdof,1,gke_sol,gdof,ipiv,gfp_sol,gdof,info)
            if (info/=0)then
                flag=.false.
                ! 求解失败,将A,B重新赋值
                print *, '      KU=F solved failed by dgesv, info = ',info
                gke_sol=gke
                gfp_sol=gf
                ok=1
            else
                flag=.true.
                gu=gfp_sol
                ok=0
                print *, '      KU=F solved by dgesv successfully !'
            end if
        end if 
        end subroutine soleq

! ----------------------------------------------------------------------
!                   modal analysis subroutines
! ----------------------------------------------------------------------
    ! subroutine do_shell_eigs_solve()
    !     use shell_mod
    !     implicit none
        
    ! end subroutine do_shell_eigs_solve
    
    !> 传入单元和边界数据,计算系统的特征值和特征向量
    subroutine do_hex_eigs_solve8(nodes,el_conn,e,nu,rho,nev,elemtype,&
                    dbc_pos,gdof,eigvals,eigvecs)
        use solid_mod,only: eldofs,nndofs,nn_el,get_dof_ind,get_ndof_inds,&
        get_eldof_inds,c3d8i_ke,c3d8_ke,c3d8_me,c3d8i_me
        implicit none
        real(real64),intent(in)     :: nodes(:,:)
        real(real64),intent(in)     :: e,nu,rho
        integer,intent(in)          :: nev
        integer,intent(in)          :: gdof
        integer,intent(in)          :: el_conn(:,:)
        integer,intent(in)          :: elemtype
        integer,intent(in)          :: dbc_pos(:,:)
        real(real64),intent(out)    :: eigvals(:)
        real(real64),intent(out)    :: eigvecs(:,:)
        ! 内部变量
        integer                     :: nnum, i, j               ! 节点数量
        integer                     :: q                        ! 子空间大小
        integer                     :: fdnum                    ! 自由自由度的数量
        integer                     :: el_inds(eldofs)          ! 单元自由度索引
        integer                     :: bcs(size(dbc_pos,1))     ! 约束的自由度数组
        integer,allocatable         :: frees(:)
        real(real64)                :: el_coords(3,nn_el)       ! 单元节点坐标
        real(real64)                :: elk(eldofs,eldofs)       ! 单元刚度矩阵
        real(real64)                :: elm(eldofs,eldofs)       ! 单元质量矩阵
        real(real64),allocatable    :: gke(:,:)                 ! 全局刚度矩阵
        real(real64),allocatable    :: gme(:,:)                 ! 全局质量矩阵
        real(real64),allocatable    :: t_mat(:,:)
        real(real64),allocatable    :: gke_(:,:)
        real(real64),allocatable    :: gme_(:,:)
        real(real64),allocatable    :: w(:),work(:)
        integer                     :: info,lwork
        nnum=size(nodes,1)
        i=0
        j=0
        q=min(2*nev,nev+8)
        el_coords       = 0.0_real64
        elk             = 0.0_real64
        elm             = 0.0_real64
        print *, 'global dof number =',gdof
        print *, 'eigen values requested =',nev
        print *, 'START ASSEMBLY K & M MATRIX......'
        allocate(gke(gdof,gdof))
        allocate(gme(gdof,gdof))
        gke             = 0.0_real64
        gme             = 0.0_real64
        do i=1,size(el_conn,1)
            ! 获得单元坐标数组
            do j=1,size(el_conn,2)
                el_coords(1,j)=nodes(el_conn(i,j),1)
                el_coords(2,j)=nodes(el_conn(i,j),2)
                el_coords(3,j)=nodes(el_conn(i,j),3)
            end do
            if (elemtype==1) then
                call c3d8_ke(e,nu,el_coords,elk)            ! 获得c3d8单元刚度矩阵
                call c3d8_me(rho,el_coords,elm)             ! 获得c3d8单元质量矩阵
            else if (elemtype==2) then
                call c3d8i_ke(e,nu,el_coords,elk)           ! 获得c3d8i单元刚度矩阵
                call c3d8i_me(rho,el_coords,elm)             ! 获得c3d8i单元质量矩阵
            end if
            call get_eldof_inds(el_conn(i,:),el_inds)       ! 获得单元自由度索引
            gke(el_inds,el_inds)=gke(el_inds,el_inds)+elk   ! 组装全局刚度矩阵
            gme(el_inds,el_inds)=gme(el_inds,el_inds)+elm   ! 组装全局刚度矩阵
        end do
        print *, 'END ASSEMBLY K & M MATRIX......'

        ! 处理约束边界条件
        print *, 'START SORTINT BOUNDARY CONDITIONS.....'
        bcs=0.0_real64
        do i=1,size(dbc_pos,1)
            ! 计算自由度索引
            bcs(i)=get_dof_ind(dbc_pos(i,1),dbc_pos(i,2))
        end do
        print *, 'END SORTINT BOUNDARY CONDITIONS.....'

        print *, 'BCS ARE INTRODUCING TO K & M MATRIX.....'
        call get_free_dofs(gdof,bcs,frees)
        fdnum=size(frees,1)
        allocate(t_mat(gdof,fdnum))
        t_mat=0.0_real64
        do i=1,fdnum                            ! 构建T矩阵, U=T*U_FREE
            t_mat(frees(i),i)=1.0_real64
        end do
        allocate(gke_(fdnum,fdnum))             ! 构建刚度/质量矩阵的自由子矩阵
        allocate(gme_(fdnum,fdnum))
        gke_ = matmul(transpose(t_mat),matmul(gke,t_mat))
        gme_ = matmul(transpose(t_mat),matmul(gme,t_mat))

        print *, 'INTRODUCING TO K & M MATRIX...OK !'
        
        print *, 'START CALLING dsygv SUBROUTINE TO SOLVE ({K}-w^2{M}){u}={0}'
        lwork=6*fdnum-2
        allocate(w(fdnum))
        allocate(work(lwork))
        call  dsygv(1,'v','U',fdnum,gke_,fdnum,gme_,fdnum,w,work,lwork,info)
        if (info/=0) then
            print *,'dsygv subroutine failed to solve GE ,info = ',info
            stop
        end if
        print *, 'dsygv solve successful !'

        do i=1,nev
            eigvecs(:,i)=matmul(t_mat,gke_(:,i))
            eigvals(i)=w(i)
        end do

        deallocate(gke_,gme_,frees)
        deallocate(gke)
        deallocate(gme)
        deallocate(w,work,t_mat)
        print *, 'END dsygv SUBROUTINE !'
    end subroutine do_hex_eigs_solve8
    !> 约束自由度绝对不能重复
    subroutine do_hex_Generalized_alpha(nodes,el_conn,e,nu,rho,alpha,beta,&
                        elemtype, dbc_pos,dbc_val,dbc_amp,fbc_pos,fbc_val,&
                        fbc_amp,amps_array,gdof,step_time,inc_time,datfile)
        use solid_mod,only: eldofs,nndofs,nn_el,get_dof_ind,get_ndof_inds,&
                            get_eldof_inds,c3d8i_ke,c3d8_ke,c3d8_me,c3d8i_me
        implicit none
        character(len=*),intent(in) :: datfile
        real(real64),intent(in)     :: nodes(:,:)
        integer,intent(in)          :: el_conn(:,:)
        real(real64),intent(in)     :: e,nu,rho,alpha,beta,step_time,inc_time
        integer,intent(in)          :: elemtype
        integer,intent(in)          :: dbc_pos(:,:)
        real(real64),intent(in)     :: dbc_val(:)
        integer,intent(in)          :: dbc_amp(:)
        integer,intent(in)          :: fbc_pos(:,:)
        real(real64),intent(out)    :: fbc_val(:)
        integer,intent(in)          :: fbc_amp(:)
        real(real64),intent(in)     :: amps_array(:,:,:)
        integer,intent(in)          :: gdof
        
        ! internal vars
        real(real64),allocatable    :: gk_ff(:,:),gm_ff(:,:),gc_ff(:,:)     ! 全局刚度/质量/阻尼矩阵,下划线表示引入边界条件后
        real(real64),allocatable    :: gk_fc(:,:),gm_fc(:,:),gc_fc(:,:)     ! 全局刚度/质量/阻尼矩阵,下划线表示引入边界条件后
        real(real64)                :: gk(gdof,gdof),gm(gdof,gdof)     ! 全局刚度/质量
        real(real64)                :: el_coords(3,nn_el),elk(eldofs,eldofs),elm(eldofs,eldofs)  ! 单元坐标矩阵和单元质量/刚度矩阵
        integer                     :: el_inds(eldofs),n_inds(nndofs)               ! 单元自由度索引
        integer,allocatable         :: dbc_dofs(:),fbc_dofs(:)     ! 约束自由度
        real(real64),allocatable    :: gu_fold(:),gv_fold(:),ga_fold(:),gf_fold(:)
        real(real64),allocatable    :: gu_fnew(:),gv_fnew(:),ga_fnew(:),gf_fnew(:)
        real(real64)                :: gu(gdof),gv(gdof),ga(gdof)
        real(real64),allocatable    :: keff(:,:),feff(:)
        real(real64),allocatable    :: uc(:),vc(:),ac(:)
        integer,allocatable         :: frees(:)     ! 自由自由度的索引数组
        integer                     :: i,j,k,stepnum    ! 索引
        integer                     :: nnum     ! 节点数
        integer                     :: fdnum,bcnum,funit,info
        real(real64)                :: told
        real(real64),allocatable    :: gm_ff_inv(:,:)
        real(real64),parameter      :: zerotol=1.0e-4_real64

        ! 广义-alpha法的积分常数,写法参照朱超的硕士论文
        real(real64),parameter      :: cgamma=0.0_real64
        real(real64),parameter      :: cdelta=1.0_real64/3.0_real64
        real(real64),parameter      :: calpha=5.0_real64/6.0_real64
        real(real64),parameter      :: cbeta=4.0_real64/9.0_real64
        real(real64)                :: c0,c1,c2,c3,c4,c5,c6
        integer                     :: t1_index,t2_index,ok
        real(real64)                :: t1,t2,a1,a2,a
        ! 暂时变量
        real(real64)                :: f0(gdof)

        !> 变量用来计算t+(1-delta)*inc_time时间点的约束节点位移/速度/加速度
        stepnum=int(step_time/inc_time)
        nnum=size(nodes,1)

        ! 装配全局刚度/质量矩阵
        print *, 'START ASSEMBLY K & M MATRIX......'
        el_coords      = 0.0_real64
        gk             = 0.0_real64
        gm             = 0.0_real64
        do i=1,size(el_conn,1)
            ! 获得单元坐标数组
            do j=1,size(el_conn,2)
                el_coords(1,j)=nodes(el_conn(i,j),1)
                el_coords(2,j)=nodes(el_conn(i,j),2)
                el_coords(3,j)=nodes(el_conn(i,j),3)
            end do
            if (elemtype==1) then
                call c3d8_ke(e,nu,el_coords,elk)            ! 获得c3d8单元刚度矩阵
                call c3d8_me(rho,el_coords,elm)             ! 获得c3d8单元质量矩阵
            else if (elemtype==2) then
                call c3d8i_ke(e,nu,el_coords,elk)           ! 获得c3d8i单元刚度矩阵
                call c3d8i_me(rho,el_coords,elm)             ! 获得c3d8i单元质量矩阵
            end if
            call get_eldof_inds(el_conn(i,:),el_inds)       ! 获得单元自由度索引
            gk(el_inds,el_inds)=gk(el_inds,el_inds)+elk   ! 组装全局刚度矩阵
            gm(el_inds,el_inds)=gm(el_inds,el_inds)+elm   ! 组装全局刚度矩阵
        end do
        print *, 'END ASSEMBLY K & M MATRIX......'

        ! 处理约束边界条件: 分成固定自由度和强制约束自由度
        print *, 'START SORTINT BOUNDARY CONDITIONS.....'
        allocate(dbc_dofs(size(dbc_val,1)))
        do i=1,size(dbc_val,1)                  ! 遍历位移约束数组
            dbc_dofs(i)=get_dof_ind(dbc_pos(i,1),dbc_pos(i,2))
        end do
        ! 找出加载力的自由度的索引向量
        allocate(fbc_dofs(size(fbc_val,1)))
        do i=1,size(fbc_val,1)
            fbc_dofs(i)=get_dof_ind(fbc_pos(i,1),fbc_pos(i,2))
        end do
        print *, 'END SORTINT BOUNDARY CONDITIONS !'
        ! -----------------------------
        !       广义alpha法
        ! -----------------------------
        print *, 'Start G-alpha solving.....'
        call get_free_dofs(gdof,dbc_dofs,frees)
        fdnum=size(frees,1)
        bcnum=size(dbc_dofs,1)
        allocate(gk_ff(fdnum,fdnum))    ! 给自由子矩阵分配内存
        allocate(gm_ff(fdnum,fdnum))
        allocate(gc_ff(fdnum,fdnum))
        allocate(gk_fc(fdnum,bcnum))  ! 给自由-约束子矩阵分配内存
        allocate(gm_fc(fdnum,bcnum))
        allocate(gc_fc(fdnum,bcnum))
        ! 提取子矩阵
        ! do i=1,size(frees)
        !     do j=1,size(frees)
        !         gk_ff(i,j) = gk(frees(i),frees(j))
        !         gm_ff(i,j) = gm(frees(i),frees(j))
        !         gc_ff(i,j) = alpha*gm_ff(i,j)+beta*gk_ff(i,j)
        !     end do
        ! end do
        ! do i=1,size(frees)
        !     do j=1,size(dbc_dofs)
        !         gk_fc(i,j)=gk(frees(i),dbc_dofs(j))
        !         gm_fc(i,j)=gm(frees(i),dbc_dofs(j))
        !         gc_fc(i,j)=alpha*gm_fc(i,j)+beta*gk_fc(i,j)
        !     end do
        ! end do
        gk_ff=gk(frees,frees)
        gm_ff=gm(frees,frees)
        gc_ff=alpha*gm_ff+beta*gk_ff
        gk_fc=gk(frees,dbc_dofs)
        gm_fc=gm(frees,dbc_dofs)
        gc_fc=alpha*gm_fc+beta*gk_fc
        print *, 'M,K,C divided into free matrix and constraint matrix OK '

        !> 上一步的自由自由度的速度/位移/加速度
        allocate(gu_fold(fdnum),gv_fold(fdnum),ga_fold(fdnum),gf_fold(fdnum))
        allocate(gu_fnew(fdnum),gv_fnew(fdnum),ga_fnew(fdnum),gf_fnew(fdnum))
        allocate(gm_ff_inv(fdnum,fdnum))
        allocate(uc(bcnum),vc(bcnum),ac(bcnum))
        allocate(keff(fdnum,fdnum))
        allocate(feff(fdnum))
        ! -------------------------------------------------------------------
        !       1. 计算全局初始位移,速度,加速度
        ! -------------------------------------------------------------------
        gu_fold=0.0_real64   ! 给初始位移和初始速度赋初值0.0
        gv_fold=0.0_real64
        gu=0.0_real64
        gv=0.0_real64
        ga=0.0_real64
        f0=0.0_real64
        gf_fold=0.0_real64
        gf_fnew=0.0_real64
        call inverse_diagonal_d(gm_ff,gm_ff_inv,k)
        if (k/=0) print *, '! Error when calculate Mff inv matrix, k=',k
        
        ! 计算0.0s时,力加载的相关节点的取值Fm*A(0.0)
        do i=1,size(fbc_dofs,1)
            j=fbc_dofs(i)
            ! 节点力=力值*幅值点A(t)
            f0(j)=fbc_val(i)*amps_array(2,1,fbc_amp(i))
        end do
        print *, 'max in F vector at 0.0s , =>',f0
    
        ! 计算0.0s时, 0位移约束和非0位移约束的相关节点的取值Um*A(0.0),Um*dA/dt(0.0),Um*ddA/ddt(0.0)
        do i=1,size(dbc_val,1)
                j=get_dof_ind(dbc_pos(i,1),dbc_pos(i,2))
                ! 节点力=力值*幅值点A(t)
                uc(i)=dbc_val(i)*amps_array(2,1,dbc_amp(i))
                vc(i)=dbc_val(i)*amps_array(3,1,dbc_amp(i))
                ac(i)=dbc_val(i)*amps_array(4,1,dbc_amp(i))
                gu(j)=uc(i)
                gv(j)=vc(i)
        end do
        print *, 'Uc, Vc,Ac at 0.0 s','max in U = ',maxval(gu)

        ! 计算加载力和约束造成的力向量(free dof num)
        gf_fold=f0(frees)-matmul(gk_fc,uc)-matmul(gc_fc,vc)-matmul(gm_fc,ac)
        print *, 'max val in gf_fold at 0.0s ==>',maxval(gf_fold)
        ! 计算0.0s时,节点初始加速度向量(free dof num)
        call soleq(gm_ff,gf_fold,fdnum,ga_fold,ok)
        if (ok/=0) then 
            print *, 'solveq failed'
            stop
        end if
        deallocate(gm_ff_inv)
        do i=1,fdnum
            j=frees(i)
            ga(j)=ga_fold(i)
        end do
        print *, 'Initial accelaration gotted ok !'
        ! -------------------------------------------------------------
        !       1-1 将初始速度/位移/加速度场写入tecplot文件 ,采用共享拓扑
        ! -------------------------------------------------------------
        print *, 'start to write init field data into .Dat file'
        open(newunit=funit,file=datfile,status='replace',action='write',iostat=info)
        if (info/=0) print *, 'open file failed ',datfile
        write(funit,'(a)') 'TITLE = "sdyna program Dynamic Analysis Results"'
        write(funit,'(a)') 'VARIABLES = "X","Y","Z","Ux","Uy","Uz","Vx","Vy","Vz","Ax","Ay","Az"'
        write(funit,'(a,i0,a,i0,a,i0,a,f5.2)') 'ZONE T="inc-',0,'", F=FEPOINT, ET=BRICK, N=',nnum,', E=',size(el_conn,1),', SOLUTIONTIME=',0.0
        do i=1,nnum
            n_inds=get_ndof_inds(i)
            write(funit,'(3F28.10,9F30.16)') nodes(i,1),nodes(i,2),nodes(i,3),gu(n_inds(1)),gu(n_inds(2)),gu(n_inds(3)),&
            gv(n_inds(1)),gv(n_inds(2)),gv(n_inds(3)),ga(n_inds(1)),ga(n_inds(2)),ga(n_inds(3))
        end do
        do i=1,size(el_conn,1)
            write(funit,'(8I16)') el_conn(i,:)
        end do
        print *, 'End to write init field data into .Dat file'

        ! -------------------------------------------------------------------
        !       2. 计算积分常数
        ! -------------------------------------------------------------------
        c0=1.0_real64-cdelta
        c1=(1.0_real64-cgamma)/(cbeta*inc_time*inc_time)
        c2=(c0*calpha)/(cbeta*inc_time)
        c3=inc_time*c1
        c4=0.5_real64*c3*inc_time-1.0_real64
        c5=c0*(calpha/cbeta)-1.0_real64
        c6=c0*inc_time*(0.5_real64*(calpha/cbeta)-1.0_real64)
        print *, 'integration constants OK'
        
        ! 计算有效刚度矩阵
        keff=c0*gk_ff+c1*gm_ff+c2*gc_ff 
        print *, 'Eeffctive Stiffness Matrix Got !'
        ! -------------------------------------------------------------------
        !       3. 循环时间步
        ! -------------------------------------------------------------------
        ! 时间步循环求解
        told=0.0_real64
        do i=1,stepnum
            print *, 'There are Time :',told
            ! (3.1) 计算有效载荷
            told=told+(1.0_real64-cdelta)*inc_time
            ! 3.1.1 计算Ff在t+(1.0-delta)Delta处的节点力加载
            t1=real(floor(told/inc_time),real64)*inc_time
            t2=t1+inc_time
            t1_index=int(t1/inc_time)+1
            t2_index=t1_index+1
            f0=0.0_real64
            do j=1,size(fbc_val,1)
                a1=amps_array(2,t1_index,fbc_amp(j))
                a2=amps_array(2,t2_index,fbc_amp(j))
                a=told*(a2-a1)/(t2-t1)+(t2*a1-t1*a2)/(t2-t1)
                f0(fbc_dofs(j))=fbc_val(j)*a ! 节点力=力值*幅值点A(t)
            end do
            print *,'max in f0 =',maxval(f0)
            ! 3.1.2 计算t+(1.0-delta)Deltas时, 0位移约束和非0位移约束的相关节点的取值Um*A(0.0),Um*dA/dt(0.0),Um*ddA/ddt(0.0)
            do j=1,size(dbc_val,1)
                k=get_dof_ind(dbc_pos(i,1),dbc_pos(i,2))
                a1=amps_array(2,t1_index,dbc_amp(j))
                a2=amps_array(2,t2_index,dbc_amp(j))
                uc(j)=dbc_val(j)*(told*(a2-a1)/(t2-t1)+(t2*a1-t1*a2)/(t2-t1))  ! 约束位移=约束值*幅值点A(t)
                gu(k)=uc(j)

                a1=amps_array(3,t1_index,dbc_amp(j))
                a2=amps_array(3,t2_index,dbc_amp(j))
                vc(j)=dbc_val(j)*(told*(a2-a1)/(t2-t1)+(t2*a1-t1*a2)/(t2-t1))  ! 约束速度=约束值*幅值点dA(t)/dt
                gv(k)=vc(j)
                
                a1=amps_array(4,t1_index,dbc_amp(j))
                a2=amps_array(4,t2_index,dbc_amp(j))
                ac(j)=dbc_val(j)*(told*(a2-a1)/(t2-t1)+(t2*a1-t1*a2)/(t2-t1))  ! 约束加速度=约束值*幅值点ddA(t)/ddt
                ga(k)=ac(j)
            end do

            feff=(f0(frees)-matmul(gk_fc,uc)+matmul(gc_fc,vc)+matmul(gm_fc,ac))-cdelta*matmul(gk_ff,gu_fold)+&
                    c1*matmul(gm_ff,gu_fold)+c3*matmul(gm_ff,gv_fold)+c4*matmul(gm_ff,ga_fold)&
                    +c2*matmul(gc_ff,gu_fold)+c5*matmul(gc_ff,gv_fold)+c6*matmul(gm_ff,ga_fold)
            ! print *,'       - Effective Load vector done !'
            print *, 'max in feff',maxval(feff)
            ! (3.2) 计算有效载荷, 调用 lapack 求解Keff*U_new=F_eff_new
            call soleq(keff,feff,fdnum,gu_fnew,ok)
            if (ok/=0) then 
                print *, 'solveq failed'
                stop
            end if
            ! print *,'       - Ending call lapack to solve Keff*U_new=F_eff_new.....'

            ! (3.3) 更新自由加速度和速度
            ga_fnew=(gu_fnew-gu_fold)/(cbeta*inc_time*inc_time)-gv_fold/(cbeta*inc_time)-ga_fold*((0.5_real64/cbeta)-1.0_real64)
            gv_fnew=(gu_fnew-gu_fold)*(calpha/(cbeta*inc_time))+(1.0_real64-calpha/cbeta)*gv_fold+(1.0_real64-0.5_real64/calpha)*ga_fold*inc_time

            ! 3.3.1 写入到gu,gv,ga
            told=real(i,real64)*inc_time
            do j=1,fdnum
                k=frees(j)
                gu(k)=gu_fnew(j)
                gv(k)=gv_fnew(j)
                ga(k)=ga_fnew(j)
            end do
            ! print *, '      - Updating the new volocity and Accelaration OK !'
            
            ! 3.3.2 将计算结果写入到dat文件,变量不同,几何共享
            write(funit,*) ''
            write(funit,'(a,f15.8,a,i10,a,i10,a,f15.8,a)') 'ZONE T="inc-',told,'", F=FEPOINT, ET=BRICK, N=',nnum,', E=',size(el_conn,1),', SOLUTIONTIME=',told &
            ,',VARSHARELIST=([1-3]=1), CONNECTIVITYSHAREZONE=1'
            do j=1,nnum
                n_inds=get_ndof_inds(j)
                write(funit,'(9F30.15)') gu(n_inds),gv(n_inds),ga(n_inds)
            end do
            print *, '      - Print datas of this step into .dat file OK!'
            ! 更新状态,进入下一时间点
            gu_fold=gu_fnew; gv_fold=gv_fnew; ga_fold=ga_fnew;
        end do
        close(funit)
        deallocate(gk_ff,gm_ff,gc_ff)
        deallocate(gk_fc,gm_fc,gc_fc)
        deallocate(dbc_dofs,fbc_dofs)
        deallocate(gu_fold,gv_fold,ga_fold,gf_fold)
        deallocate(gu_fnew,gv_fnew,ga_fnew,gf_fnew)
        deallocate(keff,feff)
        deallocate(frees)
    end subroutine do_hex_Generalized_alpha

    !> 约束自由度绝对不能重复
    subroutine do_hex_newmark(nodes,el_conn,e,nu,rho,alpha,beta,&
                        elemtype, dbc_pos,dbc_val,dbc_amp,fbc_pos,fbc_val,&
                        fbc_amp,amps_array,gdof,step_time,inc_time,datfile)
        use solid_mod,only: eldofs,nndofs,nn_el,get_dof_ind,get_ndof_inds,&
                            get_eldof_inds,c3d8i_ke,c3d8_ke,c3d8_me,c3d8i_me
        implicit none
        character(len=*),intent(in) :: datfile
        real(real64),intent(in)     :: nodes(:,:)
        integer,intent(in)          :: el_conn(:,:)
        real(real64),intent(in)     :: e,nu,rho,alpha,beta,step_time,inc_time
        integer,intent(in)          :: elemtype
        integer,intent(in)          :: dbc_pos(:,:)
        real(real64),intent(in)     :: dbc_val(:)
        integer,intent(in)          :: dbc_amp(:)
        integer,intent(in)          :: fbc_pos(:,:)
        real(real64),intent(out)    :: fbc_val(:)
        integer,intent(in)          :: fbc_amp(:)
        real(real64),intent(in)     :: amps_array(:,:,:)
        integer,intent(in)          :: gdof
        
        ! internal vars
        ! real(real64),allocatable    :: gk_ff(:,:),gm_ff(:,:),gc_ff(:,:)     ! 全局刚度/质量/阻尼矩阵,下划线表示引入边界条件后
        ! real(real64),allocatable    :: gk_fc(:,:),gm_fc(:,:),gc_fc(:,:)     ! 全局刚度/质量/阻尼矩阵,下划线表示引入边界条件后
        real(real64)                :: gk(gdof,gdof),gm(gdof,gdof),gc(gdof,gdof)     ! 全局刚度/质量
        real(real64)                :: el_coords(3,nn_el),elk(eldofs,eldofs),elm(eldofs,eldofs)  ! 单元坐标矩阵和单元质量/刚度矩阵
        integer                     :: el_inds(eldofs),n_inds(nndofs)               ! 单元自由度索引
        integer,allocatable         :: dbc_dofs(:),fbc_dofs(:)     ! 约束自由度
        real(real64),allocatable    :: gu_fold(:),gv_fold(:),ga_fold(:),gf_fold(:)
        real(real64),allocatable    :: gu_fnew(:),gv_fnew(:),ga_fnew(:),gf_fnew(:)
        real(real64)                :: g1(gdof),g2(gdof)
        real(real64),allocatable    :: keff(:,:)
        real(real64),allocatable    :: uc(:),vc(:),ac(:)
        ! integer,allocatable         :: frees(:)     ! 自由自由度的索引数组
        integer                     :: i,j,k,stepnum    ! 索引
        integer                     :: nnum     ! 节点数
        ! integer                     :: fdnum
        integer                     :: bcnum,funit,info
        real(real64)                :: told
        ! real(real64),allocatable    :: gm_ff_inv(:,:)
        real(real64),parameter      :: zerotol=1.0e-4_real64

        ! 广义-alpha法的积分常数,写法参照朱超的硕士论文
        real(real64),parameter      :: calpha = 0.6_real64
        real(real64),parameter      :: cbeta  = 0.4_real64
        real(real64)                :: ca1,ca2,ca3,ca4,ca5,ca6
        integer                     :: ok
        ! real(real64)                :: t1,t2,a1,a2,a
        ! 暂时变量
        real(real64)                :: f0(gdof)

        !> 变量用来计算t+(1-delta)*inc_time时间点的约束节点位移/速度/加速度
        stepnum=int(step_time/inc_time)
        nnum=size(nodes,1)

        ! 装配全局刚度/质量矩阵
        print *, 'START ASSEMBLY K & M MATRIX......'
        el_coords      = 0.0_real64
        gk             = 0.0_real64
        gm             = 0.0_real64
        gc             = 0.0_real64
        do i=1,size(el_conn,1)
            ! 获得单元坐标数组
            do j=1,size(el_conn,2)
                el_coords(1,j)=nodes(el_conn(i,j),1)
                el_coords(2,j)=nodes(el_conn(i,j),2)
                el_coords(3,j)=nodes(el_conn(i,j),3)
            end do
            if (elemtype==1) then
                call c3d8_ke(e,nu,el_coords,elk)            ! 获得c3d8单元刚度矩阵
                call c3d8_me(rho,el_coords,elm)             ! 获得c3d8单元质量矩阵
            else if (elemtype==2) then
                call c3d8i_ke(e,nu,el_coords,elk)           ! 获得c3d8i单元刚度矩阵
                call c3d8i_me(rho,el_coords,elm)             ! 获得c3d8i单元质量矩阵
            end if
            call get_eldof_inds(el_conn(i,:),el_inds)       ! 获得单元自由度索引
            gk(el_inds,el_inds)=gk(el_inds,el_inds)+elk   ! 组装全局刚度矩阵
            gm(el_inds,el_inds)=gm(el_inds,el_inds)+elm   ! 组装全局刚度矩阵
            gc(el_inds,el_inds)=gc(el_inds,el_inds)+alpha*elm+beta*elk
        end do
        print *, 'END ASSEMBLY K & M MATRIX......'

        ! 处理约束边界条件: 分成固定自由度和强制约束自由度
        print *, 'START SORTINT BOUNDARY CONDITIONS.....'
        allocate(dbc_dofs(size(dbc_val,1)))
        do i=1,size(dbc_val,1)                  ! 遍历位移约束数组
            dbc_dofs(i)=get_dof_ind(dbc_pos(i,1),dbc_pos(i,2))
        end do
        ! 找出加载力的自由度的索引向量
        allocate(fbc_dofs(size(fbc_val,1)))
        do i=1,size(fbc_val,1)
            fbc_dofs(i)=get_dof_ind(fbc_pos(i,1),fbc_pos(i,2))
        end do
        print *, 'END SORTINT BOUNDARY CONDITIONS !'
        ! -----------------------------
        !       newmark法
        ! -----------------------------
        print *, 'Start newmark solving.....'

        !-----------------------------
        !       计算初始条件
        !-----------------------------
        bcnum=size(dbc_dofs,1)
        allocate(gu_fold(gdof),gv_fold(gdof),ga_fold(gdof),gf_fold(gdof))
        allocate(gu_fnew(gdof),gv_fnew(gdof),ga_fnew(gdof),gf_fnew(gdof))
        allocate(uc(bcnum),vc(bcnum),ac(bcnum))
        allocate(keff(gdof,gdof))

        ! 计算0.0s时, 0位移约束和非0位移约束的相关节点的取值Um*A(0.0),Um*dA/dt(0.0)
        gu_fold=0.0_real64; gv_fold=0.0_real64; ga_fold=0.0_real64
        do i=1,size(dbc_val,1)
                j=get_dof_ind(dbc_pos(i,1),dbc_pos(i,2))
                ! 节点力=力值*幅值点A(t)
                gu_fold(j)=dbc_val(i)*amps_array(2,1,dbc_amp(i))
                gv_fold(j)=dbc_val(i)*amps_array(3,1,dbc_amp(i))
                print *, 'dbc -',i,'=',dbc_val(i)
        end do
        print *, 'Uc, Vc,Ac at 0.0 s','max in U = ',maxval(gu_fold)

        ! 计算0.0s时,力加载的相关节点的取值Fm*A(0.0)
        f0=0.0_real64
        do i=1,size(fbc_dofs,1)
            j=fbc_dofs(i)
            ! 节点力=力值*幅值点A(t)
            f0(j)=fbc_val(i)*amps_array(2,1,fbc_amp(i))
        end do
        print *, 'max in F vector at 0.0s , =>',maxval(f0)

        ! 计算初始加速度
        f0=f0-matmul(gc,gv_fold)-matmul(gk,gu_fold)
        call soleq(gm,f0,gdof,ga_fold,ok)
        if (ok/=0) then
            print *, 'error in solve init ga'
            stop
        end if
        do i=1,size(dbc_val,1)
            j=get_dof_ind(dbc_pos(i,1),dbc_pos(i,2))
            ga_fold(j)=dbc_val(i)*amps_array(4,1,dbc_amp(i))  ! 节点力=力值*幅值点A(t)
        end do
        !-----------------------------
        !       将初始位移/速度/加速度写入dat
        !-----------------------------
        print *, 'start to write init field data into .Dat file'
        open(newunit=funit,file=datfile,status='replace',action='write',iostat=info)
        if (info/=0) print *, 'open file failed ',datfile
        write(funit,'(a)') 'TITLE = "sdyna program Dynamic Analysis Results"'
        write(funit,'(a)') 'VARIABLES = "X","Y","Z","Ux","Uy","Uz","Vx","Vy","Vz","Ax","Ay","Az"'
        write(funit,'(a,i0,a,i0,a,i0,a,f5.2)') 'ZONE T="inc-',0,'", F=FEPOINT, ET=BRICK, N=',nnum,', E=',size(el_conn,1),', SOLUTIONTIME=',0.0
        do i=1,nnum
            n_inds=get_ndof_inds(i)
            write(funit,'(3F28.10,9F30.16)') nodes(i,:),gu_fold(n_inds),gv_fold(n_inds),ga_fold(n_inds)
        end do
        do i=1,size(el_conn,1)
            write(funit,'(8I16)') el_conn(i,:)
        end do
        print *, 'End to write init field data into .Dat file'
        !-----------------------------
        !       计算积分常数
        !-----------------------------
        ca1=1.0_real64/(calpha*inc_time*inc_time)
        ca2=cbeta/(calpha*inc_time)
        ca3=1.0_real64/(calpha*inc_time)
        ca4=(1.0_real64-2.0_real64*calpha)/(2.0_real64*calpha)
        ca5=0.5_real64*inc_time*((cbeta/calpha)-2.0_real64)
        ca6=cbeta/calpha-1.0_real64
        !-----------------------------
        !       计算有效刚度
        !-----------------------------
        keff=gk+ca1*gm+ca2*gc
        !-----------------------------
        !       循环时间步
        !-----------------------------
        told    = 0.0_real64
        g1      = 0.0_real64
        g2      = 0.0_real64
        do i=1,stepnum
            print *, 'There are Time :',told
            ! (1) 计算有效载荷
            gf_fnew = 0.0_real64
            do j=1,size(fbc_val,1)
                k=get_dof_ind(fbc_pos(j,1),fbc_pos(j,2))
                gf_fnew(k)=fbc_val(j)*amps_array(2,i,fbc_amp(j)) ! 节点力=力值*幅值点A(t)
            end do
            g1=ca1*gu_fold+ca3*gv_fold+ca4*ga_fold
            g2=ca2*gu_fold+ca6*gv_fold+ca5*ga_fold
            gf_fnew=gf_fnew+matmul(gm,g1)+matmul(gc,g2)
            ! (2) 求解t+dt的位移
            call soleq(keff,gf_fnew,gdof,gu_fnew,ok)
            if (ok/=0) then 
                print *, 'solveq failed'
                stop
            end if
            ! (3) 更新加速度/速度
            ga_fnew=(gu_fnew-gu_fold)/(calpha*inc_time*inc_time)-gv_fold/(calpha*inc_time)-ca4*ga_fold
            gv_fnew=gv_fold+(1.0_real64-cbeta)*inc_time*ga_fold+cbeta*inc_time*ga_fnew

            ! (4) 将约束自由度强制赋值
            do j=1,size(dbc_val,1)
                k=get_dof_ind(dbc_pos(j,1),dbc_pos(j,2))
                ! 节点力=力值*幅值点A(t)
                gu_fnew(k)=dbc_val(j)*amps_array(2,i,dbc_amp(j))
                gv_fnew(k)=dbc_val(j)*amps_array(3,i,dbc_amp(j))
                ga_fnew(k)=dbc_val(j)*amps_array(4,i,dbc_amp(j))
            end do
            
            ! 将计算结果写入到dat文件,变量不同,几何共享
            write(funit,*) ''
            write(funit,'(a,f15.8,a,i10,a,i10,a,f15.8,a)') 'ZONE T="inc-',told,'", F=FEPOINT, ET=BRICK, N=',nnum,', E=',size(el_conn,1),', SOLUTIONTIME=',told &
            ,',VARSHARELIST=([1-3]=1), CONNECTIVITYSHAREZONE=1'
            do j=1,nnum
                n_inds=get_ndof_inds(j)
                write(funit,'(9F30.15)') gu_fnew(n_inds),gv_fnew(n_inds),ga_fnew(n_inds)
            end do
            print *, '      - Print datas of this step into .dat file OK!'

            ! (5) 迭代
            gu_fold=gu_fnew; gv_fold=gv_fnew; ga_fold=ga_fnew
            told=told+inc_time
        end do

        ! call get_free_dofs(gdof,dbc_dofs,frees)
        ! fdnum=size(frees,1)
        ! bcnum=size(dbc_dofs,1)
        ! allocate(gk_ff(fdnum,fdnum))    ! 给自由子矩阵分配内存
        ! allocate(gm_ff(fdnum,fdnum))
        ! allocate(gc_ff(fdnum,fdnum))
        ! allocate(gk_fc(fdnum,bcnum))  ! 给自由-约束子矩阵分配内存
        ! allocate(gm_fc(fdnum,bcnum))
        ! allocate(gc_fc(fdnum,bcnum))
        ! 提取子矩阵
        ! do i=1,size(frees)
        !     do j=1,size(frees)
        !         gk_ff(i,j) = gk(frees(i),frees(j))
        !         gm_ff(i,j) = gm(frees(i),frees(j))
        !         gc_ff(i,j) = alpha*gm_ff(i,j)+beta*gk_ff(i,j)
        !     end do
        ! end do
        ! do i=1,size(frees)
        !     do j=1,size(dbc_dofs)
        !         gk_fc(i,j)=gk(frees(i),dbc_dofs(j))
        !         gm_fc(i,j)=gm(frees(i),dbc_dofs(j))
        !         gc_fc(i,j)=alpha*gm_fc(i,j)+beta*gk_fc(i,j)
        !     end do
        ! end do
        ! gk_ff=gk(frees,frees)
        ! gm_ff=gm(frees,frees)
        ! gc_ff=alpha*gm_ff+beta*gk_ff
        ! gk_fc=gk(frees,dbc_dofs)
        ! gm_fc=gm(frees,dbc_dofs)
        ! gc_fc=alpha*gm_fc+beta*gk_fc
        ! print *, 'M,K,C divided into free matrix and constraint matrix OK '

        ! !> 上一步的自由自由度的速度/位移/加速度
        ! allocate(gu_fold(fdnum),gv_fold(fdnum),ga_fold(fdnum),gf_fold(fdnum))
        ! allocate(gu_fnew(fdnum),gv_fnew(fdnum),ga_fnew(fdnum),gf_fnew(fdnum))
        ! allocate(gm_ff_inv(fdnum,fdnum))
        ! allocate(uc(bcnum),vc(bcnum),ac(bcnum))
        ! allocate(keff(fdnum,fdnum))
        ! allocate(feff(fdnum))
        ! ! -------------------------------------------------------------------
        ! !       1. 计算全局初始位移,速度,加速度
        ! ! -------------------------------------------------------------------
        ! gu_fold=0.0_real64   ! 给初始位移和初始速度赋初值0.0
        ! gv_fold=0.0_real64
        ! gu=0.0_real64
        ! gv=0.0_real64
        ! ga=0.0_real64
        ! f0=0.0_real64
        ! gf_fold=0.0_real64
        ! gf_fnew=0.0_real64
        ! call inverse_diagonal_d(gm_ff,gm_ff_inv,k)
        ! if (k/=0) print *, '! Error when calculate Mff inv matrix, k=',k
        
        ! ! 计算0.0s时,力加载的相关节点的取值Fm*A(0.0)
        ! do i=1,size(fbc_dofs,1)
        !     j=fbc_dofs(i)
        !     ! 节点力=力值*幅值点A(t)
        !     f0(j)=fbc_val(i)*amps_array(2,1,fbc_amp(i))
        ! end do
        ! print *, 'max in F vector at 0.0s , =>',f0
    
        ! ! 计算0.0s时, 0位移约束和非0位移约束的相关节点的取值Um*A(0.0),Um*dA/dt(0.0),Um*ddA/ddt(0.0)
        ! do i=1,size(dbc_val,1)
        !         j=get_dof_ind(dbc_pos(i,1),dbc_pos(i,2))
        !         ! 节点力=力值*幅值点A(t)
        !         uc(i)=dbc_val(i)*amps_array(2,1,dbc_amp(i))
        !         vc(i)=dbc_val(i)*amps_array(3,1,dbc_amp(i))
        !         ac(i)=dbc_val(i)*amps_array(4,1,dbc_amp(i))
        !         gu(j)=uc(i)
        !         gv(j)=vc(i)
        ! end do
        ! print *, 'Uc, Vc,Ac at 0.0 s','max in U = ',maxval(gu)

        ! ! 计算加载力和约束造成的力向量(free dof num)
        ! gf_fold=f0(frees)-matmul(gk_fc,uc)-matmul(gc_fc,vc)-matmul(gm_fc,ac)
        ! print *, 'max val in gf_fold at 0.0s ==>',maxval(gf_fold)
        ! ! 计算0.0s时,节点初始加速度向量(free dof num)
        ! call soleq(gm_ff,gf_fold,fdnum,ga_fold,ok)
        ! if (ok/=0) then 
        !     print *, 'solveq failed'
        !     stop
        ! end if
        ! deallocate(gm_ff_inv)
        ! do i=1,fdnum
        !     j=frees(i)
        !     ga(j)=ga_fold(i)
        ! end do
        ! print *, 'Initial accelaration gotted ok !'
        ! ! -------------------------------------------------------------
        ! !       1-1 将初始速度/位移/加速度场写入tecplot文件 ,采用共享拓扑
        ! ! -------------------------------------------------------------
        ! print *, 'start to write init field data into .Dat file'
        ! open(newunit=funit,file=datfile,status='replace',action='write',iostat=info)
        ! if (info/=0) print *, 'open file failed ',datfile
        ! write(funit,'(a)') 'TITLE = "sdyna program Dynamic Analysis Results"'
        ! write(funit,'(a)') 'VARIABLES = "X","Y","Z","Ux","Uy","Uz","Vx","Vy","Vz","Ax","Ay","Az"'
        ! write(funit,'(a,i0,a,i0,a,i0,a,f5.2)') 'ZONE T="inc-',0,'", F=FEPOINT, ET=BRICK, N=',nnum,', E=',size(el_conn,1),', SOLUTIONTIME=',0.0
        ! do i=1,nnum
        !     n_inds=get_ndof_inds(i)
        !     write(funit,'(3F28.10,9F30.16)') nodes(i,1),nodes(i,2),nodes(i,3),gu(n_inds(1)),gu(n_inds(2)),gu(n_inds(3)),&
        !     gv(n_inds(1)),gv(n_inds(2)),gv(n_inds(3)),ga(n_inds(1)),ga(n_inds(2)),ga(n_inds(3))
        ! end do
        ! do i=1,size(el_conn,1)
        !     write(funit,'(8I16)') el_conn(i,:)
        ! end do
        ! print *, 'End to write init field data into .Dat file'

        ! ! -------------------------------------------------------------------
        ! !       2. 计算积分常数
        ! ! -------------------------------------------------------------------
        ! c0=1.0_real64-cdelta
        ! c1=(1.0_real64-cgamma)/(cbeta*inc_time*inc_time)
        ! c2=(c0*calpha)/(cbeta*inc_time)
        ! c3=inc_time*c1
        ! c4=0.5_real64*c3*inc_time-1.0_real64
        ! c5=c0*(calpha/cbeta)-1.0_real64
        ! c6=c0*inc_time*(0.5_real64*(calpha/cbeta)-1.0_real64)
        ! print *, 'integration constants OK'
        
        ! ! 计算有效刚度矩阵
        ! keff=c0*gk_ff+c1*gm_ff+c2*gc_ff 
        ! print *, 'Eeffctive Stiffness Matrix Got !'
        ! ! -------------------------------------------------------------------
        ! !       3. 循环时间步
        ! ! -------------------------------------------------------------------
        ! ! 时间步循环求解
        ! told=0.0_real64
        ! do i=1,stepnum
        !     print *, 'There are Time :',told
        !     ! (3.1) 计算有效载荷
        !     told=told+(1.0_real64-cdelta)*inc_time
        !     ! 3.1.1 计算Ff在t+(1.0-delta)Delta处的节点力加载
        !     t1=real(floor(told/inc_time),real64)*inc_time
        !     t2=t1+inc_time
        !     t1_index=int(t1/inc_time)+1
        !     t2_index=t1_index+1
        !     f0=0.0_real64
        !     do j=1,size(fbc_val,1)
        !         a1=amps_array(2,t1_index,fbc_amp(j))
        !         a2=amps_array(2,t2_index,fbc_amp(j))
        !         a=told*(a2-a1)/(t2-t1)+(t2*a1-t1*a2)/(t2-t1)
        !         f0(fbc_dofs(j))=fbc_val(j)*a ! 节点力=力值*幅值点A(t)
        !     end do
        !     print *,'max in f0 =',maxval(f0)
        !     ! 3.1.2 计算t+(1.0-delta)Deltas时, 0位移约束和非0位移约束的相关节点的取值Um*A(0.0),Um*dA/dt(0.0),Um*ddA/ddt(0.0)
        !     do j=1,size(dbc_val,1)
        !         k=get_dof_ind(dbc_pos(i,1),dbc_pos(i,2))
        !         a1=amps_array(2,t1_index,dbc_amp(j))
        !         a2=amps_array(2,t2_index,dbc_amp(j))
        !         uc(j)=dbc_val(j)*(told*(a2-a1)/(t2-t1)+(t2*a1-t1*a2)/(t2-t1))  ! 约束位移=约束值*幅值点A(t)
        !         gu(k)=uc(j)

        !         a1=amps_array(3,t1_index,dbc_amp(j))
        !         a2=amps_array(3,t2_index,dbc_amp(j))
        !         vc(j)=dbc_val(j)*(told*(a2-a1)/(t2-t1)+(t2*a1-t1*a2)/(t2-t1))  ! 约束速度=约束值*幅值点dA(t)/dt
        !         gv(k)=vc(j)
                
        !         a1=amps_array(4,t1_index,dbc_amp(j))
        !         a2=amps_array(4,t2_index,dbc_amp(j))
        !         ac(j)=dbc_val(j)*(told*(a2-a1)/(t2-t1)+(t2*a1-t1*a2)/(t2-t1))  ! 约束加速度=约束值*幅值点ddA(t)/ddt
        !         ga(k)=ac(j)
        !     end do

        !     feff=(f0(frees)-matmul(gk_fc,uc)+matmul(gc_fc,vc)+matmul(gm_fc,ac))-cdelta*matmul(gk_ff,gu_fold)+&
        !             c1*matmul(gm_ff,gu_fold)+c3*matmul(gm_ff,gv_fold)+c4*matmul(gm_ff,ga_fold)&
        !             +c2*matmul(gc_ff,gu_fold)+c5*matmul(gc_ff,gv_fold)+c6*matmul(gm_ff,ga_fold)
        !     ! print *,'       - Effective Load vector done !'
        !     print *, 'max in feff',maxval(feff)
        !     ! (3.2) 计算有效载荷, 调用 lapack 求解Keff*U_new=F_eff_new
        !     call soleq(keff,feff,fdnum,gu_fnew,ok)
        !     if (ok/=0) then 
        !         print *, 'solveq failed'
        !         stop
        !     end if
        !     ! print *,'       - Ending call lapack to solve Keff*U_new=F_eff_new.....'

        !     ! (3.3) 更新自由加速度和速度
        !     ga_fnew=(gu_fnew-gu_fold)/(cbeta*inc_time*inc_time)-gv_fold/(cbeta*inc_time)-ga_fold*((0.5_real64/cbeta)-1.0_real64)
        !     gv_fnew=(gu_fnew-gu_fold)*(calpha/(cbeta*inc_time))+(1.0_real64-calpha/cbeta)*gv_fold+(1.0_real64-0.5_real64/calpha)*ga_fold*inc_time

        !     ! 3.3.1 写入到gu,gv,ga
        !     told=real(i,real64)*inc_time
        !     do j=1,fdnum
        !         k=frees(j)
        !         gu(k)=gu_fnew(j)
        !         gv(k)=gv_fnew(j)
        !         ga(k)=ga_fnew(j)
        !     end do
        !     ! print *, '      - Updating the new volocity and Accelaration OK !'
            
        !     ! 3.3.2 将计算结果写入到dat文件,变量不同,几何共享
        !     write(funit,*) ''
        !     write(funit,'(a,f15.8,a,i10,a,i10,a,f15.8,a)') 'ZONE T="inc-',told,'", F=FEPOINT, ET=BRICK, N=',nnum,', E=',size(el_conn,1),', SOLUTIONTIME=',told &
        !     ,',VARSHARELIST=([1-3]=1), CONNECTIVITYSHAREZONE=1'
        !     do j=1,nnum
        !         n_inds=get_ndof_inds(j)
        !         write(funit,'(9F30.15)') gu(n_inds),gv(n_inds),ga(n_inds)
        !     end do
        !     print *, '      - Print datas of this step into .dat file OK!'
        !     ! 更新状态,进入下一时间点
        !     gu_fold=gu_fnew; gv_fold=gv_fnew; ga_fold=ga_fnew;
        ! end do
        ! close(funit)
        ! deallocate(gk_ff,gm_ff,gc_ff)
        ! deallocate(gk_fc,gm_fc,gc_fc)
        ! deallocate(dbc_dofs,fbc_dofs)
        ! deallocate(gu_fold,gv_fold,ga_fold,gf_fold)
        ! deallocate(gu_fnew,gv_fnew,ga_fnew,gf_fnew)
        ! deallocate(keff,feff)
        ! deallocate(frees)
    end subroutine do_hex_newmark


    !> 输入总自由度数gdof,约束自由度索引数组bcs(:),返回自由自由度索引数组
    subroutine get_free_dofs(gdof,bcs,frees)
        implicit none
        integer, intent(in)  :: gdof,bcs(:)
        integer, allocatable :: frees(:)
        logical, allocatable :: mask(:)             ! 掩码数组
        integer :: i, cnt,k
        k=size(bcs,1)
        ! 1. 建立掩码：True 表示保留
        allocate(mask(gdof))
        mask = .true.                               ! 先全开
        do i = 1, k
            if (bcs(i) >= 1 .and. bcs(i) <= gdof) mask(bcs(i)) = .false.
        end do

        ! 2. 统计保留个数，以便一次性分配
        cnt = count(mask)                           ! Fortran 内建，极快
        allocate(frees(cnt))

        ! 3. 把保留元素写进结果
        cnt = 0
        do i = 1, gdof
            if (mask(i)) then
            cnt = cnt + 1
            frees(cnt) = i
            end if
        end do
    end subroutine get_free_dofs
end module ls_mod